/*
    Combine multiple messages into one single message

    #33,#74 -> #184
    #0,#1,#24,#30 -> #185
*/

#include "augmavconn/msgcombine.h"
#include <mavlink/v1.0/common/mavlink.h>

// typedef struct __mavlink_global_position_int_t {
//  uint32_t time_boot_ms; /*< Timestamp (milliseconds since system boot)*/
//  int32_t lat; /*< Latitude, expressed as degrees * 1E7*/
//  int32_t lon; /*< Longitude, expressed as degrees * 1E7*/
//  int32_t alt; /*< Altitude in meters, expressed as * 1000 (millimeters), AMSL (not WGS84 - note that virtually all GPS modules provide the AMSL as well)*/
//  int32_t relative_alt; /*< Altitude above ground in meters, expressed as * 1000 (millimeters)*/
//  int16_t vx; /*< Ground X Speed (Latitude, positive north), expressed as m/s * 100*/
//  int16_t vy; /*< Ground Y Speed (Longitude, positive east), expressed as m/s * 100*/
//  int16_t vz; /*< Ground Z Speed (Altitude, positive down), expressed as m/s * 100*/
//  uint16_t hdg; /*< Vehicle heading (yaw angle) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/
// }) mavlink_global_position_int_t;

// MAVPACKED(
// typedef struct __mavlink_vfr_hud_t {
//  float airspeed; /*< Current airspeed in m/s*/
//  float groundspeed; /*< Current ground speed in m/s*/
//  float alt; /*< Current altitude (MSL), in meters*/
//  float climb; /*< Current climb rate in meters/second*/
//  int16_t heading; /*< Current heading in degrees, in compass units (0..360, 0=north)*/
//  uint16_t throttle; /*< Current throttle setting in integer percent, 0 to 100*/
// }) mavlink_vfr_hud_t;

//184
// MAVPACKED(
// typedef struct __mavlink_state_combined_5hz_t {
//  uint32_t Timestamp; /*< Timestamp (milliseconds since system boot)*/
//  int32_t lat; /*< Latitude, expressed as degrees * 1E7*/
//  int32_t lon; /*< Longitude, expressed as degrees * 1E7*/
//  int32_t alt; /*< Altitude in meters, expressed as * 1000 (millimeters), AMSL (not WGS84 - note that virtually all GPS modules provide the AMSL as well)*/
//  int32_t relative_alt; /*< Altitude above ground in meters, expressed as * 1000 (millimeters)*/
//  float airspeed; /*< Current airspeed in m/s*/
//  float groundspeed; /*< Current ground speed in m/s*/
//  int16_t vx; /*< Ground X Speed (Latitude, positive north), expressed as m/s * 100*/
//  int16_t vy; /*< Ground Y Speed (Longitude, positive east), expressed as m/s * 100*/
//  int16_t vz; /*< Ground Z Speed (Altitude, positive down), expressed as m/s * 100*/
//  uint16_t hdg; /*< Vehicle heading (yaw angle) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/
//  int16_t heading; /*< Current heading in degrees, in compass units (0..360, 0=north)*/
//  uint16_t throttle; /*< Current throttle setting in integer percent, 0 to 100*/
// }) mavlink_state_combined_5hz_t;

void combineMsg184(mavlink_message_t* message184, const mavlink_message_t *message33, const mavlink_message_t * message74)
{
    //decode #33 and #74 messages
    mavlink_global_position_int_t global_position_int; //33
    mavlink_vfr_hud_t  vfr_hud; //74
    mavlink_state_combined_5hz_t state_combined_5hz;  //184

    // currently use the first message's sysid and compid  
    uint8_t system_id=2;//message33->sysid;
    uint8_t component_id=2; //message33->compid;

    mavlink_msg_global_position_int_decode(message33,&global_position_int);
    //mavlink_msg_global_position_int_encode(system_id,component_id,message184,&global_position_int);
    mavlink_msg_vfr_hud_decode(message74,&vfr_hud);

    //assemble new message
    state_combined_5hz.Timestamp = global_position_int.time_boot_ms;

    state_combined_5hz.lat = global_position_int.lat;
    state_combined_5hz.lon = global_position_int.lon;
    state_combined_5hz.alt = global_position_int.alt;
    state_combined_5hz.relative_alt = global_position_int.relative_alt;
    state_combined_5hz.vx = global_position_int.vx;
    state_combined_5hz.vy = global_position_int.vy;
    state_combined_5hz.vz = global_position_int.vz;
    state_combined_5hz.hdg = global_position_int.hdg;
    
    state_combined_5hz.airspeed = vfr_hud.airspeed;
    state_combined_5hz.groundspeed = vfr_hud.groundspeed;
    state_combined_5hz.heading = vfr_hud.heading;
    state_combined_5hz.throttle = vfr_hud.throttle;

    mavlink_msg_state_combined_5hz_encode(system_id,component_id,message184,&state_combined_5hz);

    /*mavlink_msg_state_combined_5hz_pack(
    system_id,
    component_id,

    message184,

    state_combined_5hz.Timestamp ,
    
    state_combined_5hz.lat ,
    state_combined_5hz.lon ,
    state_combined_5hz.alt ,
    state_combined_5hz.relative_alt ,
    state_combined_5hz.vx ,
    state_combined_5hz.vy ,
    state_combined_5hz.vz ,
    state_combined_5hz.hdg ,
    
    state_combined_5hz.airspeed ,
    state_combined_5hz.groundspeed ,
    state_combined_5hz.heading ,
    state_combined_5hz.throttle 

    );*/
}
//0
// MAVPACKED(
// typedef struct __mavlink_heartbeat_t {
//  uint32_t custom_mode; /*< A bitfield for use for autopilot-specific flags.*/
//  uint8_t type; /*< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)*/
//  uint8_t autopilot; /*< Autopilot type / class. defined in MAV_AUTOPILOT ENUM*/
//  uint8_t base_mode; /*< System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h*/
//  uint8_t system_status; /*< System status flag, see MAV_STATE ENUM*/
//  uint8_t mavlink_version; /*< MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version*/
// }) mavlink_heartbeat_t;


//1
// MAVPACKED(
// typedef struct __mavlink_sys_status_t {
//  uint32_t onboard_control_sensors_present; /*< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR*/
//  uint32_t onboard_control_sensors_enabled; /*< Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR*/
//  uint32_t onboard_control_sensors_health; /*< Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR*/
//  uint16_t load; /*< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000*/
//  uint16_t voltage_battery; /*< Battery voltage, in millivolts (1 = 1 millivolt)*/
//  int16_t current_battery; /*< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current*/
//  uint16_t drop_rate_comm; /*< Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)*/
//  uint16_t errors_comm; /*< Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)*/
//  uint16_t errors_count1; /*< Autopilot-specific errors*/
//  uint16_t errors_count2; /*< Autopilot-specific errors*/
//  uint16_t errors_count3; /*< Autopilot-specific errors*/
//  uint16_t errors_count4; /*< Autopilot-specific errors*/
//  int8_t battery_remaining; /*< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery*/
// }) mavlink_sys_status_t;


//24
// typedef struct __mavlink_gps_raw_int_t {
//  uint64_t time_usec; /*< Timestamp (microseconds since UNIX epoch or microseconds since system boot)*/
//  int32_t lat; /*< Latitude (WGS84), in degrees * 1E7*/
//  int32_t lon; /*< Longitude (WGS84), in degrees * 1E7*/
//  int32_t alt; /*< Altitude (AMSL, NOT WGS84), in meters * 1000 (positive for up). Note that virtually all GPS modules provide the AMSL altitude in addition to the WGS84 altitude.*/
//  uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX*/
//  uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX*/
//  uint16_t vel; /*< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX*/
//  uint16_t cog; /*< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/
//  uint8_t fix_type; /*< See the GPS_FIX_TYPE enum.*/
//  uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to 255*/
// }) mavlink_gps_raw_int_t;

//30 

// MAVPACKED(
// typedef struct __mavlink_attitude_t {
//  uint32_t time_boot_ms; /*< Timestamp (milliseconds since system boot)*/
//  float roll; /*< Roll angle (rad, -pi..+pi)*/
//  float pitch; /*< Pitch angle (rad, -pi..+pi)*/
//  float yaw; /*< Yaw angle (rad, -pi..+pi)*/
//  float rollspeed; /*< Roll angular speed (rad/s)*/
//  float pitchspeed; /*< Pitch angular speed (rad/s)*/
//  float yawspeed; /*< Yaw angular speed (rad/s)*/
// }) mavlink_attitude_t;

void combineMsg185(mavlink_message_t * message185, const mavlink_message_t *message0, const mavlink_message_t * message1, const mavlink_message_t * message24, const mavlink_message_t * message30){
    mavlink_heartbeat_t heartbeat;
    mavlink_sys_status_t sys_status;
    mavlink_gps_raw_int_t gps_raw_int;    
    mavlink_attitude_t attitude;
    mavlink_state_combined_1hz_t state_combined_1hz;

    // currently use the first message's sysid and compid  
    uint8_t system_id=message0->sysid;
    uint8_t component_id= message0->compid;
    
    mavlink_msg_heartbeat_decode(message0, &heartbeat);
    mavlink_msg_sys_status_decode(message1, &sys_status);
    mavlink_msg_gps_raw_int_decode(message24, &gps_raw_int);
    mavlink_msg_attitude_decode(message30,&attitude);
    
    //STATE_COMBINED_1HZ
    state_combined_1hz.Timestamp=gps_raw_int.time_usec; 
    state_combined_1hz.custom_mode=heartbeat.custom_mode; 
    state_combined_1hz.roll=attitude.roll; 
    state_combined_1hz.pitch=attitude.pitch; 
    state_combined_1hz.yaw=attitude.yaw; 
    state_combined_1hz.voltage_battery=sys_status.voltage_battery; 
    state_combined_1hz.cog=gps_raw_int.cog; 
    state_combined_1hz.base_mode=heartbeat.base_mode; 
    state_combined_1hz.fix_type=gps_raw_int.fix_type; 
    state_combined_1hz.satellites_visible=gps_raw_int.satellites_visible; 


    mavlink_msg_state_combined_1hz_pack(
    system_id,
    component_id,

    message185,

    state_combined_1hz.Timestamp, 
    state_combined_1hz.base_mode,
    state_combined_1hz.custom_mode,
    state_combined_1hz.voltage_battery, 
    state_combined_1hz.fix_type, 
    state_combined_1hz.cog, 
    state_combined_1hz.satellites_visible, 
    state_combined_1hz.roll, 
    state_combined_1hz.pitch, 
    state_combined_1hz.yaw

    );
}
